#include "SharedSettings.h"
#include "CMatrix3d.h"


const fixed CMatrix3d::LookAtUpVectorTolerance = CMatrix3d_LookAtUpVectorTolerance;


CMatrix3d::CMatrix3d()
{
	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = 0; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix3d::CMatrix3d( const fixed* m )
{ 
	ASSERT( m ); 

	MEMCPY( this, m, sizeof( CMatrix3d ) );

	ASSERT( !m_orientationFixupIsApplicable || 
			( m_orientationFixupIsApplicable && m_nOfArthmeticOpsTriggeringOrientationFixup ) );
}

 
CMatrix3d::CMatrix3d( fixed m00, fixed m01, fixed m02, 
					  fixed m10, fixed m11, fixed m12,
					  fixed m20, fixed m21, fixed m22,
					  int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	m_col0.m_i = m00; 
	m_col0.m_j = m01;
	m_col0.m_k = m02;
	m_col1.m_i = m10; 
	m_col1.m_j = m11;
	m_col1.m_k = m12;
	m_col2.m_i = m20; 
	m_col2.m_j = m21;
	m_col2.m_k = m22;

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix3d::CMatrix3d( const CVector3d* col, int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	MEMCPY( this, col, sizeof( CVector3d ) * 3 );

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix3d::CMatrix3d( const CVector3d& col0, const CVector3d& col1, const CVector3d& col2,
					  int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	m_col0 = col0; 
	m_col1 = col1; 
	m_col2 = col2; 

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix3d::CMatrix3d( const CMatrix4d& m )
{
	m_col0 = *(const CVector3d*)&m.m_col0;
	m_col1 = *(const CVector3d*)&m.m_col1;
	m_col2 = *(const CVector3d*)&m.m_col2;
	
	m_orientationFixupIsApplicable = m.m_orientationFixupIsApplicable;
	m_nOfArithmeticOpsSinceLastFixup = m.m_nOfArithmeticOpsSinceLastFixup;
	m_nOfArthmeticOpsTriggeringOrientationFixup = m.m_nOfArthmeticOpsTriggeringOrientationFixup;
}


CMatrix3d::CMatrix3d( const CMatrix4dh& m )
{
	m_col0 = *(const CVector3d*)&m.m_col0;
	m_col1 = *(const CVector3d*)&m.m_col1;
	m_col2 = *(const CVector3d*)&m.m_col2;

	m_orientationFixupIsApplicable = m.m_orientationFixupIsApplicable;
	m_nOfArithmeticOpsSinceLastFixup = m.m_nOfArithmeticOpsSinceLastFixup;
	m_nOfArthmeticOpsTriggeringOrientationFixup = m.m_nOfArthmeticOpsTriggeringOrientationFixup;
}


CMatrix3d& CMatrix3d::LoadIdentity()
{
	m_col0.m_i = CMathFixed::One;
	m_col0.m_j = 0;
	m_col0.m_k = 0;

	m_col1.m_i = 0;							
	m_col1.m_j = CMathFixed::One;	
	m_col1.m_k = 0;

	m_col2.m_i = 0;
	m_col2.m_j = 0;
	m_col2.m_k = CMathFixed::One;

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


CMatrix3d& CMatrix3d::Orthonormalize()
{
	CVector3d b0( m_col0.m_i, m_col1.m_i, m_col2.m_i );
	CVector3d b1( m_col0.m_j, m_col1.m_j, m_col2.m_j );

	b0.Normalize();

	fixed dOrtho = b0 * b1;

	if( dOrtho )
	{
		b1.m_i -= CMathFixed::Mul( b0.m_i, dOrtho );
		b1.m_j -= CMathFixed::Mul( b0.m_j, dOrtho );
		b1.m_k -= CMathFixed::Mul( b0.m_k, dOrtho );
	}
		
	b1.Normalize();
	
	CVector3d b2( b0 );

	b2.Cross( b1 );

	m_col0.Set( b0.m_i, b1.m_i, b2.m_i );
	m_col1.Set( b0.m_j, b1.m_j, b2.m_j );
	m_col2.Set( b0.m_k, b1.m_k, b2.m_k );

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


CMatrix3d& CMatrix3d::RotateX( fixed degrees )
{
	fixed* a = (fixed*)this;
	fixed a3 = a[3];
	fixed a4 = a[4];
	fixed a5 = a[5];

	fixed cos = CMathFixed::Cos( degrees );
	fixed sin = CMathFixed::Sin( degrees );

	a[3] = CMathFixed::Mul( a3, cos ) + CMathFixed::Mul( a[6], sin );
	a[4] = CMathFixed::Mul( a4, cos ) + CMathFixed::Mul( a[7], sin );
	a[5] = CMathFixed::Mul( a5, cos ) + CMathFixed::Mul( a[8], sin );

	a[6] = CMathFixed::Mul( a3, -sin ) + CMathFixed::Mul( a[6], cos );
	a[7] = CMathFixed::Mul( a4, -sin ) + CMathFixed::Mul( a[7], cos );
	a[8] = CMathFixed::Mul( a5, -sin ) + CMathFixed::Mul( a[8], cos );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix3d&	CMatrix3d::RotateY( fixed degrees )
{
	fixed* a = (fixed*)this;
	fixed a0 = a[0];
	fixed a1 = a[1];
	fixed a2 = a[2];

	fixed cos = CMathFixed::Cos( degrees );
	fixed sin = CMathFixed::Sin( degrees );

	a[0] = CMathFixed::Mul( a0, cos ) + CMathFixed::Mul( a[6], -sin );
	a[1] = CMathFixed::Mul( a1, cos ) + CMathFixed::Mul( a[7], -sin );
	a[2] = CMathFixed::Mul( a2, cos ) + CMathFixed::Mul( a[8], -sin );

	a[6] = CMathFixed::Mul( a0, sin ) + CMathFixed::Mul( a[6], cos );
	a[7] = CMathFixed::Mul( a1, sin ) + CMathFixed::Mul( a[7], cos );
	a[8] = CMathFixed::Mul( a2, sin ) + CMathFixed::Mul( a[8], cos );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix3d&	CMatrix3d::RotateZ( fixed degrees )
{
	fixed* a = (fixed*)this;
	fixed a0 = a[0];
	fixed a1 = a[1];
	fixed a2 = a[2];

	fixed cos = CMathFixed::Cos( degrees );
	fixed sin = CMathFixed::Sin( degrees );

	a[0] = CMathFixed::Mul( a0, cos ) + CMathFixed::Mul( a[3], sin );
	a[1] = CMathFixed::Mul( a1, cos ) + CMathFixed::Mul( a[4], sin );
	a[2] = CMathFixed::Mul( a2, cos ) + CMathFixed::Mul( a[5], sin );

	a[3] = CMathFixed::Mul( a0, -sin ) + CMathFixed::Mul( a[3], cos );
	a[4] = CMathFixed::Mul( a1, -sin ) + CMathFixed::Mul( a[4], cos );
	a[5] = CMathFixed::Mul( a2, -sin ) + CMathFixed::Mul( a[5], cos );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix3d&	CMatrix3d::Rotate( fixed degrees, fixed i, fixed j, fixed k )
{
	CMatrix3d B;
	fixed* b = (fixed*)&B;
	fixed s = CMathFixed::Sin( degrees );
	fixed omc = CMathFixed::One - CMathFixed::Cos( degrees );
	fixed i2 = CMathFixed::Mul( i, i );
	fixed j2 = CMathFixed::Mul( j, j );
	fixed k2 = CMathFixed::Mul( k, k );
	fixed ij = CMathFixed::Mul( i, j );
	fixed ik = CMathFixed::Mul( i, k );
	fixed jk = CMathFixed::Mul( j, k );	
	fixed si = CMathFixed::Mul( s, i );
	fixed sj = CMathFixed::Mul( s, j );
	fixed sk = CMathFixed::Mul( s, k );
	fixed omcij = CMathFixed::Mul( omc, ij ); 
	fixed omcik = CMathFixed::Mul( omc, ik ); 
	fixed omcjk = CMathFixed::Mul( omc, jk ); 

	b[0] = CMathFixed::One + CMathFixed::Mul( omc, -k2-j2 );
	b[1] = sk + omcij;
	b[2] = -sj + omcik;

	b[3] = -sk + omcij;
	b[4] = CMathFixed::One + CMathFixed::Mul( omc, -k2-i2 );
	b[5] = si + omcjk;

	b[6] = sj + omcik;
	b[7] = -si + omcjk;
	b[8] = CMathFixed::One + CMathFixed::Mul( omc, -j2-i2 );

	*this *= B;

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix3d& CMatrix3d::Translate( fixed i, fixed j, fixed k ) 
{ 
	//+todo
	ASSERT( FALSE ); // Not yet implemented

	m_orientationFixupIsApplicable = FALSE;

	return *this; 
}


CMatrix3d& CMatrix3d::LookAtRHUpJ( fixed i, fixed j, fixed k )
{
	CVector3d l( i, j, k );
	l.Normalize();

	m_col0.m_k = l.m_i;
	m_col1.m_k = l.m_j;
	m_col2.m_k = l.m_k;

	CVector3d v;
	fixed Upj = CMathFixed::Mul( l.m_j, l.m_j );

	if( Upj < CMatrix3d::LookAtUpVectorTolerance )
	{
		v.m_j = CMathFixed::One - Upj;
		v.m_i = -CMathFixed::Mul( l.m_j, l.m_i );
		v.m_k = -CMathFixed::Mul( l.m_j, l.m_k );

		v.Normalize();

		m_col0.m_j = v.m_i;
		m_col1.m_j = v.m_j;
		m_col2.m_j = v.m_k;

		v.Cross( l );

		m_col0.m_i = v.m_i;
		m_col1.m_i = v.m_j;
		m_col2.m_i = v.m_k;	
	}
	else
	{
		v.m_i = CMathFixed::One - CMathFixed::Mul( l.m_i, l.m_i );
		v.m_j = -CMathFixed::Mul( l.m_i, l.m_j );
		v.m_k = -CMathFixed::Mul( l.m_i, l.m_k );

		v.Normalize();

		m_col0.m_i = v.m_i;
		m_col1.m_i = v.m_j;
		m_col2.m_i = v.m_k;

		l.Cross( v );

		m_col0.m_j = l.m_i;
		m_col1.m_j = l.m_j;
		m_col2.m_j = l.m_k;
	}

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


CMatrix3d& CMatrix3d::LookAtRHUpK( fixed i, fixed j, fixed k )
{
	CVector3d l( i, j, k );
	l.Normalize();

	m_col0.m_j = l.m_i;
	m_col1.m_j = l.m_j;
	m_col2.m_j = l.m_k;

	CVector3d v;
	fixed Upk = CMathFixed::Mul( l.m_k, l.m_k );

	if( Upk < CMatrix3d::LookAtUpVectorTolerance )
	{
		v.m_i = -CMathFixed::Mul( l.m_k, l.m_i );
		v.m_j = -CMathFixed::Mul( l.m_k, l.m_j );
		v.m_k = CMathFixed::One - Upk;

		v.Normalize();

		m_col0.m_k = v.m_i;
		m_col1.m_k = v.m_j;
		m_col2.m_k = v.m_k;

		l.Cross( v );

		m_col0.m_i = l.m_i;
		m_col1.m_i = l.m_j;
		m_col2.m_i = l.m_k;	
	}
	else
	{
		v.m_i = CMathFixed::One - CMathFixed::Mul( l.m_i, l.m_i );
		v.m_j = -CMathFixed::Mul( l.m_i, l.m_j );
		v.m_k = -CMathFixed::Mul( l.m_i, l.m_k );

		v.Normalize();

		m_col0.m_i = v.m_i;
		m_col1.m_i = v.m_j;
		m_col2.m_i = v.m_k;

		v.Cross( l );

		m_col0.m_k = v.m_i;
		m_col1.m_k = v.m_j;
		m_col2.m_k = v.m_k;
	}

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


CMatrix3d& CMatrix3d::Transpose()
{
	fixed* a = (fixed*)this;
	fixed a1 = a[1];
	fixed a2 = a[2];
	//fixed a4 = a[3];
	fixed a5 = a[6];

	a[1] = a[3]; a[2] = a[6]; 

	a[3] = a1; a[5] = a[7];

	a[6] = a2; a[7] = a5;

	return *this;
}


CMatrix3d& CMatrix3d::operator*=( const CMatrix3d& b )
{
	fixed* a = (fixed*)this;
	fixed a0 = a[0];
	fixed a1 = a[1];
	fixed a2 = a[2];
	fixed a3 = a[3];
	fixed a4 = a[5];
	fixed a5 = a[5];

	a[0] = CMathFixed::Mul( a0, b[0] ) + CMathFixed::Mul( a3, b[1] ) + CMathFixed::Mul( a[6], b[2] );
	a[1] = CMathFixed::Mul( a1, b[0] ) + CMathFixed::Mul( a4, b[1] ) + CMathFixed::Mul( a[7], b[2] );
	a[2] = CMathFixed::Mul( a2, b[0] ) + CMathFixed::Mul( a5, b[1] ) + CMathFixed::Mul( a[8], b[2] );

	a[3] = CMathFixed::Mul( a0, b[3] ) + CMathFixed::Mul( a3, b[4] ) + CMathFixed::Mul( a[6], b[5] );
	a[4] = CMathFixed::Mul( a1, b[3] ) + CMathFixed::Mul( a4, b[4] ) + CMathFixed::Mul( a[7], b[5] );
	a[5] = CMathFixed::Mul( a2, b[3] ) + CMathFixed::Mul( a5, b[4] ) + CMathFixed::Mul( a[8], b[5] );

	a[6] = CMathFixed::Mul( a0, b[6] ) + CMathFixed::Mul( a3, b[7] ) + CMathFixed::Mul( a[6], b[8] );
	a[7] = CMathFixed::Mul( a1, b[6] ) + CMathFixed::Mul( a4, b[7] ) + CMathFixed::Mul( a[7], b[8] );
	a[8] = CMathFixed::Mul( a2, b[6] ) + CMathFixed::Mul( a5, b[7] ) + CMathFixed::Mul( a[8], b[8] );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix3d operator*( const CMatrix3d& a, const CMatrix3d& b )
{
	return CMatrix3d(
			CMathFixed::Mul( a[0], b[0] ) + CMathFixed::Mul( a[3], b[1] ) + CMathFixed::Mul( a[6], b[2] ),
			CMathFixed::Mul( a[1], b[0] ) + CMathFixed::Mul( a[4], b[1] ) + CMathFixed::Mul( a[7], b[2] ),
			CMathFixed::Mul( a[2], b[0] ) + CMathFixed::Mul( a[5], b[1] ) + CMathFixed::Mul( a[8], b[2] ),

			CMathFixed::Mul( a[0], b[3] ) + CMathFixed::Mul( a[3], b[4] ) + CMathFixed::Mul( a[6], b[5] ),
			CMathFixed::Mul( a[1], b[3] ) + CMathFixed::Mul( a[4], b[4] ) + CMathFixed::Mul( a[7], b[5] ),
			CMathFixed::Mul( a[2], b[3] ) + CMathFixed::Mul( a[5], b[4] ) + CMathFixed::Mul( a[8], b[5] ),

			CMathFixed::Mul( a[0], b[6] ) + CMathFixed::Mul( a[3], b[7] ) + CMathFixed::Mul( a[6], b[8] ),
			CMathFixed::Mul( a[1], b[6] ) + CMathFixed::Mul( a[4], b[7] ) + CMathFixed::Mul( a[7], b[8] ),
			CMathFixed::Mul( a[2], b[6] ) + CMathFixed::Mul( a[5], b[7] ) + CMathFixed::Mul( a[8], b[8] ),
			
			a.GetNumOfArithmeticOpsTriggeringOrientationFixup() );
}


inline
void CMatrix3d::IncrementOperationCount()
{
	if( m_nOfArithmeticOpsSinceLastFixup < MAX_UINT16 )
		m_nOfArithmeticOpsSinceLastFixup++;
}


inline
void CMatrix3d::ConsiderOrientationFixup()
{
	if( m_orientationFixupIsApplicable &&
		m_nOfArithmeticOpsSinceLastFixup >= m_nOfArthmeticOpsTriggeringOrientationFixup )
		Orthonormalize();
}
